import rclpy
from rclpy.node import Node
import rclpy.qos
from rclpy.parameter import Parameter
from sensor_msgs.msg import LaserScan
import math
from geometry_msgs.msg import PoseStamped
import matplotlib # type: ignore
matplotlib.use('TkAgg')
import numpy as np # type: ignore
import matplotlib.pyplot as plt # type: ignore
import sys

class ObstacleDetector(Node):
    def __init__(self):
        super().__init__('obstacle_detector')
        # self.scan_sub = rospy.Subscriber('/iris_7/scan', LaserScan, self.scan_callback)
        # self.pose_sub = rospy.Subscriber('/iris_7/mavros/local_position/pose', PoseStamped, self.pose_callback)
        self.uav_id = int(sys.argv[1])
        self.obstacle_threshold = 0.01
        self.obstacles = []
        self.local_pose = PoseStamped()
        self.obstacle_positions = []
        # self.set_parameter(Parameter("obs_position", Parameter.Type.DOUBLE_ARRAY, self.obstacle_positions))
        # 创建一个参数描述符，指定类型为 DOUBLE_ARRAY
        # param_descriptor = ParameterDescriptor(type=Parameter.Type.DOUBLE_ARRAY)
        self.declare_parameter('calculated_obstacles')           # 创建一个参数，并设置参数的默认值
        qos_profile_1 = rclpy.qos.QoSProfile(
        reliability=rclpy.qos.ReliabilityPolicy.BEST_EFFORT, 
        history=rclpy.qos.HistoryPolicy.KEEP_LAST,              
        depth=10
        )
        self.scan_sub = self.create_subscription(
            LaserScan,  # 消息类型
            f'/drone{self.uav_id}/scan',  # 话题名称
            self.scan_callback,  # 回调函数
            qos_profile_1
        )
        self.pose_sub = self.create_subscription(
            PoseStamped,  # 消息类型
            f'/drone{self.uav_id}/local_position/pose',  # 话题名称
            self.pose_callback,  # 回调函数
            qos_profile_1
        )
        # 打印启动日志
        self.get_logger().info("'obstacle_detector' node has been started!")

    def distance(self, x1, y1, x2, y2):
        return math.sqrt((x2 - x1)**2 + (y2 - y1)**2)

    def aggregate_obstacles(self):
        aggregated_obstacles = []

        for obstacle in self.obstacles:
            if not aggregated_obstacles:
                aggregated_obstacles.append([obstacle])
            else:
                added_to_existing = False
                for group in aggregated_obstacles:
                    if not group:
                        continue
                    x_avg = sum([pt[0] for pt in group]) / len(group)
                    y_avg = sum([pt[1] for pt in group]) / len(group)
                    if not (math.isinf(x_avg) or math.isinf(y_avg)):
                        if self.distance(x_avg, y_avg, obstacle[0], obstacle[1]) <= self.obstacle_threshold:
                            group.append(obstacle)
                            added_to_existing = True
                            break
                if not added_to_existing:
                    aggregated_obstacles.append([obstacle])

        return aggregated_obstacles

    def pose_callback(self, pose_msg):
        self.local_pose = pose_msg

    def scan_callback(self, scan_msg):
        angle_min = scan_msg.angle_min
        angle_increment = scan_msg.angle_increment
        range_min = scan_msg.range_min
        self.obstacles = []
        pose_x = []
        pose_y = []
        pose_z = []

        robot_x = self.local_pose.pose.position.x
        robot_y = self.local_pose.pose.position.y
        robot_yaw = self.get_yaw_from_quaternion(self.local_pose.pose.orientation)-math.pi/2
        print(f"yaw:{robot_yaw}")
        # Iterate through each range measurement in the scan
        for i, range_measurement in enumerate(scan_msg.ranges):
            # Calculate the angle of the current range measurement
            angle = angle_min + i * angle_increment- robot_yaw

            # Skip invalid measurements (out of range or NaN)
            if range_measurement <= range_min or math.isnan(range_measurement):
                continue

            # Skip obstacles that are behind the robot
            if angle < 0-robot_yaw or angle > 2*math.pi-robot_yaw:
                #print(f"range:{angle}")
                continue

            # Calculate the Cartesian coordinates of the obstacle relative to the laser
            x = range_measurement * math.cos(angle)
            y = range_measurement * math.sin(angle)
            pose_x.append(x)
            pose_y.append(y)
            pose_z.append(4.0)
            # Append the obstacle's (x, y) position to the list
            self.obstacles.append((x, y))

        # Aggregate nearby obstacles
        aggregated_obstacles = self.aggregate_obstacles()

        # Get the robot's current position and orientation
        

        # Clear previous obstacle positions
        self.obstacle_positions = []
        expansion_radius = 1.0  # The expansion radius (1 meter)
        print("this is obstacle_positions")
        # Print or process the aggregated obstacles
        for group in aggregated_obstacles:
            if not group:
                continue
            x_avg = sum([pt[0] for pt in group]) / len(group)
            y_avg = sum([pt[1] for pt in group]) / len(group)
            if not (math.isinf(x_avg) or math.isinf(y_avg)):
                # Transform obstacle coordinates from laser frame to world frame
                world_x = x_avg * math.cos(angle) - y_avg * math.sin(angle) 
                world_y = x_avg * math.sin(angle) + y_avg * math.cos(angle)

                self.obstacle_positions.append(world_x)
                self.obstacle_positions.append(world_y)
                self.obstacle_positions.append(4.0)
                # pose_x.append(world_x)
                # pose_y.append(world_y)
                # pose_z.append(4.0)
        # ====动画显示====
        # print(k)
        area = self.compute_area(0, 0, 0, 10) #暂时修改为二维
        ArrowLength = 0.5  # 箭头长度
        plt.ion()  # 打开交互模式
        plt.clf()
        # for j in range(N - 1):
        #     plt.quiver(pose_x[j, k + 1], pose_y[j, k + 1], ArrowLength * np.cos(pose_th[j, k + 1]),
        #             ArrowLength * np.sin(pose_th[j, k + 1]), scale=1, scale_units='xy', color='k',
        #             width=0.01)  # 绘制机器人的方向
        #     if j == N:
        #         state = 2  # 领导者标记
        #     else:
        #         state = 1  # 跟随者标记
        #     self.draw_circle(pose_x[j, k + 1], pose_y[j, k + 1], 0.25, state)  # 绘制机器人
        #     # for i in range(N):
        #     # if A[i, j] == 1:
        #     # draw_arrow([pose_x[j, k+1], pose_y[j, k+1]], [pose_x[i, k+1], pose_y[i, k+1]], 0.2)  # 绘制连接箭头，显示通信或控制关系
        plt.plot(robot_x, robot_y, 'Xb', linewidth=2)
        if len(pose_x) != 0:
            plt.plot(pose_x[:], pose_y[:], '.k', linewidth=2)  # 绘制障碍物位置

        plt.axis(area)  # 设置显示区域
        plt.grid(True)  # 显示网格
        plt.draw()  # 实时更新图形
        plt.pause(0.001)  # 添加短暂的暂停以显示图形
        # plt.show()
        # 判断是否到达目标
        # now = [pose_x[N - 1, k + 1], pose_y[N - 1, k + 1]]  # 获取领导者当前位置
        # difference_now = [now[i] - goal[i] for i in range(len(now))]
        # if np.linalg.norm(difference_now) < 0.5:
        #     print('Arrive Goal!!')  # 如果领导者到达目标点附近，则显示到达信息并结束循环
        # arrive_time+=1
        # if arrive_time%3==0 and path_times<path_len-1:
        # path_times+=1
        plt.ioff()  # 关闭交互模式   
                #self.obstacle_positions.append((world_x, world_y))
                # Expand the obstacle by adding points in a circular area with radius of expansion_radius
                # num_points = 8  # Number of points to generate around the obstacle (circle)
                # for i in range(num_points):
                #     for j in range(10):
                #         angle_offset = 2 * math.pi * i / num_points
                #         expanded_x = world_x + expansion_radius * 0.1*(j+1)*math.cos(angle_offset)
                #         expanded_y = world_y + expansion_radius * 0.1*(j+1)*math.sin(angle_offset)
                #         # self.obstacle_positions.append((expanded_x, expanded_y,4.0))
                #         self.obstacle_positions.append(expanded_x)
                #         self.obstacle_positions.append(expanded_y)
                #         self.obstacle_positions.append(4.0)
                #rospy.set_param("/obs_x", float(world_x))
                #rospy.set_param("/obs_y", float(world_y))
        # rospy.set_param("/obs_position", self.obstacle_positions)
        # self.set_parameter(Parameter("obs_position", Parameter.Type.DOUBLE_ARRAY, self.obstacle_positions))
        obstacle_positions_param = Parameter('calculated_obstacles',  # 重新将参数值设置为指定值
                            Parameter.Type.DOUBLE_ARRAY, self.obstacle_positions)
        self.set_parameters([obstacle_positions_param])                    # 将重新创建的参数列表发送给ROS2系统
        # print(self.obstacle_positions)
    def get_yaw_from_quaternion(self, quaternion):
        # Convert quaternion to yaw angle
        q = quaternion
        siny_cosp = 2 * (q.w * q.z + q.x * q.y)
        cosy_cosp = 1 - 2 * (q.y * q.y + q.z * q.z)
        yaw = math.atan2(siny_cosp, cosy_cosp)
        return yaw
    def compute_area(self, x, y, z, range=10):  
        '''计算动画显示的区域'''
        if x >= 0:
            num_x = np.floor(x / range)
            x1 = num_x * range
            x2 = (num_x + 1) * range
        else:
            num_x = np.floor(x / -range)
            x1 = -range * (num_x + 1)
            x2 = -range * num_x
        if y >= 0:
            num_y = np.floor(y / range)
            y1 = num_y * range
            y2 = (num_y + 1) * range
        else:
            num_y = np.floor(y / -range)
            y1 = -range * (num_y + 1)
            y2 = -range * num_y
        if z >= 0:
            num_z = np.floor(z / range)
            z1 = num_z * range
            z2 = (num_z + 1) * range
        else:
            num_z = np.floor(z / -range)
            z1 = -range * (num_z + 1)
            z2 = -range * num_z
        # area = [x1 - 8, x2 + 8, y1 - 8, y2 + 8, z1 -8, z2 + 8]
        area = [x1 - 8, x2, y1 - 8, y2 ]
        return area
def main(args=None):
    rclpy.init(args=args)
    
    try:
        detector = ObstacleDetector()
        rclpy.spin(detector)  
    except KeyboardInterrupt:
        pass
    finally:
        rclpy.shutdown()  

if __name__ == '__main__':
    main()
